#include "logger.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Transform.h"
#include <unordered_map>

class LoggerNode : public rclcpp::Node
{
  public:
	std::vector<std::string> input_topics;

	std::string output_path = "";
	std::shared_ptr<FileLogger> output;
	std::vector<bool> input_updated;
	std::vector<geometry_msgs::msg::Pose> input_poses;
	std::vector<rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr> path_subs;
	std::unordered_map<std::string, int> input_idx;

	void write_one_line(double time)
	{
		std::vector<double> line;
		line.push_back(time);
		// x y yaw
		double yaw, temp;
		for (auto p : input_poses)
		{
			line.push_back(p.position.x);
			line.push_back(p.position.y);
			tf2::Quaternion q(p.orientation.x, p.orientation.y, p.orientation.z,
							  p.orientation.w);
			tf2::Transform tf(q, tf2::Vector3(0, 0, 0));
			tf.getBasis().getEulerYPR(yaw, temp, temp);
			line.push_back(yaw);
		}
		this->output->write_line(line);
		RCLCPP_INFO(this->get_logger(), "put one line to file");
	}

	void path_callback(nav_msgs::msg::Path::SharedPtr path, const std::string &topic)
	{

		if (input_idx.find(topic) == input_idx.end())
		{
			RCLCPP_ERROR(this->get_logger(), "unexpected topic subscription (%s)",
						 topic.c_str());
			return;
		}
		RCLCPP_INFO(this->get_logger(), "call back on topic %s", topic.c_str());
		// update latest pose of the topic
		int idx = input_idx[topic];
		input_updated[idx] = true;
		input_poses[idx] = path->poses.back().pose;
		// check if all topics are updated
		for (auto updated : input_updated)
		{
			if (!updated) return;
		}
		// all updated, write this line with latest timestamp
		write_one_line(rclcpp::Time(path->header.stamp).seconds());
		for (unsigned long i = 0; i < input_topics.size(); i++)
		{
			input_updated[i] = false;
		}
	}

	LoggerNode(std::string name) : Node(name)
	{
		this->declare_parameter<std::vector<std::string>>("input_topics", input_topics);
		this->get_parameter("input_topics", input_topics);

		if (input_topics.size() < 1)
		{
			RCLCPP_ERROR(this->get_logger(), "no input topic given, exit");
			exit(EXIT_FAILURE);
		}
		RCLCPP_INFO(this->get_logger(),
					"list of input topics as below (total %ld):", input_topics.size());
		for (auto s : input_topics)
		{
			RCLCPP_INFO(this->get_logger(), s.c_str());
		}
		this->declare_parameter("output_path", output_path);
		this->get_parameter("output_path", output_path);
		if (output_path == "")
		{
			output_path = "~/pathlog.txt";
		}
		RCLCPP_INFO(this->get_logger(), "will write output to %s", output_path.c_str());
		this->output = std::make_shared<FileLogger>(output_path);
		std::vector<std::string> head;
		head.push_back("time");
		for (auto s : input_topics)
		{
			head.push_back(s + ".x");
			head.push_back(s + ".y");
			head.push_back(s + ".yaw");
		}
		this->output->write_line(head);

		path_subs.resize(input_topics.size());
		input_updated.resize(input_topics.size());
		input_poses.resize(input_topics.size());
		for (unsigned long i = 0; i < input_topics.size(); i++)
		{
			path_subs[i] = this->create_subscription<nav_msgs::msg::Path>(
				input_topics[i], 100,
				[this, i](const nav_msgs::msg::Path::SharedPtr path){
					const std::string topic_name = path_subs[i]->get_topic_name();
					path_callback(path, topic_name);
				});
			input_updated[i] = false;
			input_idx[path_subs[i]->get_topic_name()] = i;
		}
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<LoggerNode>("path_logger");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
